#include "stdio.h"
#include "stdint.h"
#include "string.h"
#include "saeJ1939.h"


/* 支持并行收发、解析的句柄 */
J1939PackType g_typeJ1939Packs[J1939_PACK_NUMBER];

/* 支持的 PGN 链表 */
J1939Fuction g_typeJ1939FuctionList;


/* PGN注册函数 */
int8_t cJ1939PgnCreat(uint32_t uiPGN, void *pvFuction)
{
    J1939Fuction *ptypeJ1939Fuction;

    for(ptypeJ1939Fuction = &g_typeJ1939FuctionList; ptypeJ1939Fuction->next != NULL; ptypeJ1939Fuction = ptypeJ1939Fuction->next);

    if((ptypeJ1939Fuction->next = pvJ1939Malloc(sizeof(J1939Fuction))) == NULL)
        return -1;

    ptypeJ1939Fuction = ptypeJ1939Fuction->next;

    ptypeJ1939Fuction->PGN = uiPGN;
    ptypeJ1939Fuction->fuction = pvFuction;
    ptypeJ1939Fuction->next = NULL;

    return 0;
}

int8_t cJ1939TpCmSend(uint32_t uiPGN, uint8_t ucDA, uint8_t ucControl, uint32_t uiPacketSize, uint32_t uiFrameNo, uint32_t uiFrameCnt)
{
    J1939PduType typeJ1939Pdu = {.P = 3, .R = 0, .DP = 0, .PF = PGN_TP_CM >> 8, .PS = ucDA, .length = 8};

    typeJ1939Pdu.SA = J1939_LOCAL_ADDRESS;

    memset(typeJ1939Pdu.datas, 0xFF, sizeof(typeJ1939Pdu.datas));
    typeJ1939Pdu.datas[0] = ucControl;
    typeJ1939Pdu.datas[5] = uiPGN;
    typeJ1939Pdu.datas[6] = uiPGN >> 8;
    typeJ1939Pdu.datas[7] = uiPGN >> 16;

    switch(ucControl)
    {
        case CONTROL_BYTE_TP_CM_RTS:
        case CONTROL_BYTE_TP_CM_BAM:
        case CONTROL_BYTE_TP_CM_EndOfMsgACK:
            typeJ1939Pdu.datas[1] = uiPacketSize;
            typeJ1939Pdu.datas[2] = uiPacketSize >> 8;
            typeJ1939Pdu.datas[3] = (uiPacketSize / 7) + ((uiPacketSize % 7) != 0);
            break;

        case CONTROL_BYTE_TP_CM_CTS:
            typeJ1939Pdu.datas[1] = uiFrameNo;
            typeJ1939Pdu.datas[2] = uiFrameCnt;
            break;

        case CONTROL_BYTE_TP_CM_ABORT:
            break;

        default : break;
    }

    return cJ1939PduSend(&typeJ1939Pdu);
}

int8_t cJ1939TpCmAbortSend(uint32_t uiPGN, uint8_t ucDA)
{
    return cJ1939TpCmSend(uiPGN, ucDA, CONTROL_BYTE_TP_CM_ABORT, 0, 0, 0);
}

int8_t cJ1939TpCmBamSend(uint32_t uiPGN, uint32_t uiPacketSize)
{
    return cJ1939TpCmSend(uiPGN, 0xFF, CONTROL_BYTE_TP_CM_BAM, uiPacketSize, 0, 0);
}

int8_t cJ1939TpCmEndOfMsgAckSend(uint32_t uiPGN, uint8_t ucDA, uint32_t uiPacketSize)
{
    return cJ1939TpCmSend(uiPGN, ucDA, CONTROL_BYTE_TP_CM_EndOfMsgACK, uiPacketSize, 0, 0);
}

int8_t cJ1939TpCmCtsSend(uint32_t uiPGN, uint8_t ucDA, uint32_t uiFrameNo, uint32_t uiFrameCnt)
{
    return cJ1939TpCmSend(uiPGN, ucDA, CONTROL_BYTE_TP_CM_CTS, 0, uiFrameNo, uiFrameCnt);
}

int8_t cJ1939TpCmRtsSend(uint32_t uiPGN, uint8_t ucDA, uint32_t uiPacketSize)
{
    return cJ1939TpCmSend(uiPGN, ucDA, CONTROL_BYTE_TP_CM_RTS, uiPacketSize, 0, 0);
}

int8_t cJ1939TpDtSends(J1939PackType *ptypeJ1939Pack)
{
    J1939PduType typeJ1939Pdu = {.P = 7, .R = 0, .DP = 0, .PF = PGN_TP_DT >> 8, .length = 8};
    int32_t iLength;
    int8_t cError = 0;

    if((ptypeJ1939Pack == NULL) || (ptypeJ1939Pack->frameRequestNum < 1))
        return -1;

    typeJ1939Pdu.PS = ptypeJ1939Pack->SA;
    typeJ1939Pdu.SA = J1939_LOCAL_ADDRESS;

    while((ptypeJ1939Pack->frameRequestNum--) > 0)
    {
        /* 帧长度 */
        iLength = ptypeJ1939Pack->length - (ptypeJ1939Pack->frameNo - 1) * 7;
        iLength = (iLength > 7) ? 7 : ((iLength < 0) ? 0 : iLength);

        memset(typeJ1939Pdu.datas, 0xFF, sizeof(typeJ1939Pdu.datas));

        typeJ1939Pdu.datas[0] = ptypeJ1939Pack->frameNo;
        memcpy(&typeJ1939Pdu.datas[1], &ptypeJ1939Pack->datas[(ptypeJ1939Pack->frameNo - 1) * 7], iLength);

        ++ptypeJ1939Pack->frameNo;

        cError |= cJ1939PduSend(&typeJ1939Pdu);
    }

    return cError;
}

int8_t cJ1939AckSend(uint32_t uiPGN, uint8_t ucDA, uint8_t uiControl)
{
    J1939PduType typeJ1939Pdu = {.P = 6, .R = 0, .DP = 0, .PF = PGN_ACKNOWLEDGEMENT >> 8, .PS = ucDA, .length = 8};

    typeJ1939Pdu.SA = J1939_LOCAL_ADDRESS;

    memset(typeJ1939Pdu.datas, 0xFF, sizeof(typeJ1939Pdu.datas));
    typeJ1939Pdu.datas[0] = uiControl;
    typeJ1939Pdu.datas[5] = uiPGN;
    typeJ1939Pdu.datas[6] = uiPGN >> 8;
    typeJ1939Pdu.datas[7] = uiPGN >> 16;

    return cJ1939PduSend(&typeJ1939Pdu);
}

int8_t cJ1939PackSend(J1939PackType *ptypeJ1939Pack, uint8_t ucDA)
{
    J1939PduType typeJ1939Pdu = {.R = 0, .DP = 0, .length = 8};

    typeJ1939Pdu.P  = ptypeJ1939Pack->P;
    typeJ1939Pdu.PF =  ptypeJ1939Pack->PGN >> 8;
    typeJ1939Pdu.PS = ((ptypeJ1939Pack->PGN >> 8) < 0xF0) ? ucDA : ptypeJ1939Pack->PGN;
    typeJ1939Pdu.SA = J1939_LOCAL_ADDRESS;

    memset(typeJ1939Pdu.datas, 0xFF, sizeof(typeJ1939Pdu.datas));
    memcpy(typeJ1939Pdu.datas, ptypeJ1939Pack->datas, ptypeJ1939Pack->length);

    return cJ1939PduSend(&typeJ1939Pdu);
}

/* PGN 被动请求 */
int8_t cJ1939Response(J1939PackType *ptypeJ1939Pack, J1939PduType *ptypeJ1939Pdu)
{
    J1939Fuction *ptypeJ1939Fuction;
    uint32_t uiPGN;
    int8_t cError = -1;

    uiPGN  = (((uint32_t)ptypeJ1939Pdu->datas[2] & 0x03) << 16) + ((uint32_t)ptypeJ1939Pdu->datas[1] << 8);
    uiPGN += (ptypeJ1939Pdu->datas[1] < 0xF0) ? 0 : ptypeJ1939Pdu->datas[0];

    for(ptypeJ1939Fuction = g_typeJ1939FuctionList.next; ptypeJ1939Fuction != NULL; ptypeJ1939Fuction = ptypeJ1939Fuction->next)
    {
        if(ptypeJ1939Fuction->PGN == uiPGN)
        {
            if(ptypeJ1939Fuction->fuction != NULL)
            {
                cError = ptypeJ1939Fuction->fuction(ptypeJ1939Pack, 1);
            }

            break;
        }
    }

    if(cError == 0)
    {
        /* 回复 ACK */
        cJ1939AckSend(uiPGN, (ptypeJ1939Pdu->PS == 0xFF) ? 0xFF : ptypeJ1939Pdu->SA, CONTROL_BYTE_ACKNOWLEDGEMENT_PGN_SUPPORTED);

        /* TP.DT 多帧发送 */
        if(ptypeJ1939Pack->length > 8)
        {
            /* 以链接管理的形式回复 */
            cJ1939TpCmRtsSend(ptypeJ1939Pack->PGN, ptypeJ1939Pdu->SA, ptypeJ1939Pack->length);
        }
        /* 单帧发送 */
        else
        {
            /* 回复 PDU */
            cJ1939PackSend(ptypeJ1939Pack, (ptypeJ1939Pdu->PS == 0xFF) ? 0xFF : ptypeJ1939Pdu->SA);
        }
    }
    /* 此PGN没有支持 */
    else
    {
        /* 非全局地址才回复 */
        if(ptypeJ1939Pdu->PS != 0xFF)
        {
            /* 回复 NACK */
            cJ1939AckSend(uiPGN, ptypeJ1939Pdu->SA, CONTROL_BYTE_ACKNOWLEDGEMENT_PGN_NOT_SUPPORTED);
        }
    }

    return cError;
}

/* PGN 主动发送 */
int8_t cJ1939PgnSend(uint32_t uiPGN, uint8_t ucDA)
{
    J1939PackType *ptypeJ1939Pack = &g_typeJ1939Packs[2];
    J1939Fuction *ptypeJ1939Fuction;
    int8_t cError = -1;

    for(ptypeJ1939Fuction = g_typeJ1939FuctionList.next; ptypeJ1939Fuction != NULL; ptypeJ1939Fuction = ptypeJ1939Fuction->next)
    {
        if(ptypeJ1939Fuction->PGN == uiPGN)
        {
            if(ptypeJ1939Fuction->fuction != NULL)
            {
                cError = ptypeJ1939Fuction->fuction(ptypeJ1939Pack, 1);
            }

            break;
        }
    }

    if(cError != 0)
        return -2;

    /* TP.DT 多帧发送 */
    if(ptypeJ1939Pack->length > 8)
    {
        /* 以链接管理的形式发送 */
        if((ptypeJ1939Pack->PGN & 0xFF00) < 0xF000)
        {
            cJ1939TpCmRtsSend(ptypeJ1939Pack->PGN, ucDA, ptypeJ1939Pack->length);
        }
        /* 以广播的形式发送 */
        else
        {
            cJ1939TpCmBamSend(ptypeJ1939Pack->PGN, ptypeJ1939Pack->length);

            ptypeJ1939Pack->SA = 0xFF;
            ptypeJ1939Pack->frameRequestNum = (ptypeJ1939Pack->length / 7) + ((ptypeJ1939Pack->length % 7) != 0);
            ptypeJ1939Pack->frameNo = 1;
            cJ1939TpDtSends(ptypeJ1939Pack);
        }
    }
    /* 单帧发送 */
    else
    {
        /* 发送 PDU */
        cJ1939PackSend(ptypeJ1939Pack, ucDA);
    }

    return cError;
}

/* PGN 被动接收 */
int8_t cJ1939PgnReceive(J1939PackType *ptypeJ1939Pack)
{
    J1939Fuction *ptypeJ1939Fuction;
    int8_t cError = -1;

    for(ptypeJ1939Fuction = g_typeJ1939FuctionList.next; ptypeJ1939Fuction != NULL; ptypeJ1939Fuction = ptypeJ1939Fuction->next)
    {
        if(ptypeJ1939Fuction->PGN == ptypeJ1939Pack->PGN)
        {
            if(ptypeJ1939Fuction->fuction != NULL)
            {
                cError = ptypeJ1939Fuction->fuction((void *)ptypeJ1939Pack, 0);
            }

            break;
        }
    }

    return cError;
}

/* PDU 被动接收 */
int8_t cJ1939PduReceive(J1939PackType *ptypeJ1939Pack, J1939PduType *ptypeJ1939Pdu)
{
    uint32_t uiPGN = (ptypeJ1939Pdu->datas[7] << 16) + (ptypeJ1939Pdu->datas[6] << 8) + ptypeJ1939Pdu->datas[5];
    int32_t iLength, iOffset, i;

    printf("cJ1939PduReceive P: %d, DP: %d, PF: %02X, PS: %02X, SA: %02X, length: %d, datas:",
                                                                                            ptypeJ1939Pdu->P,
                                                                                            ptypeJ1939Pdu->DP,
                                                                                            ptypeJ1939Pdu->PF,
                                                                                            ptypeJ1939Pdu->PS,
                                                                                            ptypeJ1939Pdu->SA,
                                                                                            ptypeJ1939Pdu->length);
    for(i = 0; i < ptypeJ1939Pdu->length; ++i)
    {
        printf(" %02X", ptypeJ1939Pdu->datas[i]);
    }
    printf("\r\n");

    switch((uint32_t)ptypeJ1939Pdu->PF << 8)
    {
        case PGN_TP_CM:
            switch(ptypeJ1939Pdu->datas[0])
            {
                case CONTROL_BYTE_TP_CM_RTS :
                    iLength = ((uint32_t)ptypeJ1939Pdu->datas[2] << 8) + ptypeJ1939Pdu->datas[1];
                    if((ptypeJ1939Pack->state != STATE_TP_CM_IDLE) || (iLength > sizeof(ptypeJ1939Pack->datas)))
                    {
                        cJ1939TpCmAbortSend(uiPGN, ptypeJ1939Pdu->SA);
                        ptypeJ1939Pack->state = STATE_TP_CM_IDLE;
                        break;
                    }

                    ptypeJ1939Pack->SA = ptypeJ1939Pdu->SA;
                    ptypeJ1939Pack->PGN = uiPGN;
                    ptypeJ1939Pack->length = iLength;
                    ptypeJ1939Pack->frameNo = 0;
                    cJ1939TpCmCtsSend(uiPGN, ptypeJ1939Pdu->SA, ptypeJ1939Pack->frameNo, 1);
                    ptypeJ1939Pack->state = STATE_TP_DT_WAIT;
                break;

                case CONTROL_BYTE_TP_CM_CTS :
                    if(ptypeJ1939Pack->state == STATE_TP_CM_IDLE)
                    {
                        break;
                    }

                    if(ptypeJ1939Pdu->datas[1] != 0)
                    {
                        /* 请求的数据帧超出范围 */
                        if((ptypeJ1939Pdu->datas[1] + ptypeJ1939Pdu->datas[2]) > (sizeof(ptypeJ1939Pack->datas) / 7 + 1))
                        {
                            cJ1939TpCmAbortSend(uiPGN, ptypeJ1939Pdu->SA);
                            ptypeJ1939Pack->state = STATE_TP_CM_IDLE;
                            break;
                        }

                        ptypeJ1939Pack->frameRequestNum = ptypeJ1939Pdu->datas[1];
                        ptypeJ1939Pack->frameNo = ptypeJ1939Pdu->datas[2];
                        cJ1939TpDtSends(ptypeJ1939Pack);
                        ptypeJ1939Pack->state = STATE_TP_CM_CTS_WAIT;
                    }
                break;

                case CONTROL_BYTE_TP_CM_BAM :
                    iLength = ((uint32_t)ptypeJ1939Pdu->datas[2] << 8) + ptypeJ1939Pdu->datas[1];
                    if((ptypeJ1939Pack->state != STATE_TP_CM_IDLE) || (iLength > sizeof(ptypeJ1939Pack->datas)))
                    {
                        ptypeJ1939Pack->state = STATE_TP_CM_IDLE;
                        break;
                    }

                    ptypeJ1939Pack->SA = ptypeJ1939Pdu->SA;
                    ptypeJ1939Pack->PGN = uiPGN;
                    ptypeJ1939Pack->length = iLength;
                    ptypeJ1939Pack->frameNo = 0;
                    ptypeJ1939Pack->state = STATE_TP_CM_ABORT_WAIT;
                break;

                case CONTROL_BYTE_TP_CM_ABORT :
                case CONTROL_BYTE_TP_CM_EndOfMsgACK :
                    ptypeJ1939Pack->state = STATE_TP_CM_IDLE;
                break;

                default : break;
            }
        break;

        case PGN_TP_DT:
            iOffset = (ptypeJ1939Pdu->datas[0] - 1) * 7;

            /* 长度限制，防止溢出 */
            if(ptypeJ1939Pdu->length > (ptypeJ1939Pack->length - iOffset))
                return -2;

            memcpy(&ptypeJ1939Pack->datas[iOffset], &ptypeJ1939Pdu->datas[1], ptypeJ1939Pdu->length);

            ptypeJ1939Pack->frameNo = ptypeJ1939Pdu->datas[0];
            ptypeJ1939Pack->frameRequestNum--;

            /* 接收完成 */
            if(ptypeJ1939Pack->frameNo > (ptypeJ1939Pack->length / 7))
            {
                /* 非广播模式，才回复ACK */
                if(ptypeJ1939Pack->state == STATE_TP_CM_ABORT_WAIT)
                {
                    cJ1939TpCmEndOfMsgAckSend(uiPGN, ptypeJ1939Pdu->SA, ptypeJ1939Pack->length);
                }

                /* 解析接收到的数据包 */
                cJ1939PgnReceive(ptypeJ1939Pack);
                ptypeJ1939Pack->state = STATE_TP_CM_IDLE;
            }
            /* 再次唤起接收 */
            else if((ptypeJ1939Pack->frameRequestNum < 1) && (ptypeJ1939Pack->state == STATE_TP_DT_WAIT))
            {
                cJ1939TpCmCtsSend(uiPGN, ptypeJ1939Pdu->SA, ptypeJ1939Pack->frameNo, 1);
                ptypeJ1939Pack->state = STATE_TP_DT_WAIT;
            }
        break;

        case PGN_REQUEST:
            cJ1939Response(ptypeJ1939Pack, ptypeJ1939Pdu);
        break;

        default :
            ptypeJ1939Pack->PGN = ((uint32_t)ptypeJ1939Pdu->DP << 16) | ((uint32_t)ptypeJ1939Pdu->PF << 8);
            ptypeJ1939Pack->PGN |= (ptypeJ1939Pdu->PF < 0xF0) ? 0 : ptypeJ1939Pdu->PS;
            ptypeJ1939Pack->SA = ptypeJ1939Pdu->SA;
            ptypeJ1939Pack->length = ptypeJ1939Pdu->length;
            memcpy(ptypeJ1939Pack->datas, ptypeJ1939Pdu->datas, ptypeJ1939Pdu->length);

            /* 解析接收到的数据包 */
            cJ1939PgnReceive(ptypeJ1939Pack);
        break;
    }

    return 0;
}
